#include "CHaptik3dofPointer.h"
#include "CWorld.h"

#pragma warning(disable: 4244)
#pragma warning(disable: 4996)

//Scale of the proxy radius for collisions
#define CHAI_SCALE_PROXY_RADIUS 0.001f

cHaptik3dofPointer::cHaptik3dofPointer(cWorld* a_world)
{
    m_waitForSmallForce = true;

    m_world = a_world;

    //Force algorithm (PROXY ALGO)
    cProxyPointForceAlgo* default_proxy = new cProxyPointForceAlgo;
    m_pointForceAlgos.push_back(default_proxy);

    //Init
    default_proxy->initialize(m_world, cVector3d(0,0,0));
    default_proxy->setProxyRadius(m_displayRadius * CHAI_SCALE_PROXY_RADIUS);

	//Phisycal device connected
	m_device = new cHaptikDevice();

    //Rendering characteristics of the tool
	//Color of the "virtual" with force-feedback
    m_colorDevice.set(1.0f, 0.0f, 0.0f);
	//Color with button on
    m_colorProxyButtonOn.set(1.0f, 0.4f, 0.0f);
	//Color of the device/proxy or of the proxy with force-feedback
    m_colorProxy.set(0.8f, 0.6f, 0.0f);
	//Color of the line
    m_colorLine.set(0.7f, 0.7f, 0.7f);
	m_render_mode = RENDER_PROXY_AND_DEVICE;

    //Radius of the device and proxy
    setRadius(0.05f);
    
    //ToolFrame
    m_showToolFrame = true;
    m_toolFrameSize = 1.0;

    //Forces
    m_forceON = true;
    m_forceStarted = false;

	//Workspace
    m_halfWorkspaceAxisX = 0.1;
    m_halfWorkspaceAxisY = 0.1;
    m_halfWorkspaceAxisZ = 0.1;

    //Init position variables
    m_deviceGlobalPos.zero();
    m_deviceLocalPos.zero();
    m_lastComputedLocalForce.zero();
    m_lastComputedGlobalForce.zero();
    m_deviceLocalVel.zero();
    m_deviceGlobalVel.zero();

    //Use normalized positions
    m_useNormalizedPositions = true;

	//Device controls in the workspace
	m_hapticsEnabled = 0;
	m_toolReady = 0;
	m_deviceEnabled = 0;
	m_toolInPause = false;
}

cHaptik3dofPointer::~cHaptik3dofPointer()
{
	if(m_device){
	if(m_device->m_systemAvailable){
     m_device->close();
	 m_device->release();
	}

    delete m_device;
	}
}

//Start Haptik Library for the devices list
int cHaptik3dofPointer::startHaptikLibrary(){

	return m_device->startHaptik();
}

//Choose a particular haptic device from the list
int cHaptik3dofPointer::getHaptikDevice(UINT32 a_deviceInput){

	int result = 0;

	result = m_device->getHaptikDevice(a_deviceInput);
	
	assignDevice();
	
	return result;
}

//Assign the device
void cHaptik3dofPointer::assignDevice(){

	//Control if the device has been activated
	m_systemAvailable = m_device->m_systemAvailable;

    if (m_systemAvailable)
    {
		//Haptic device used
		m_physicalDevice = m_device->getId();
		//Memorize also the scale factor for space for future use
		m_workspaceScale.x = m_device->m_scalePositionX;
		m_workspaceScale.y = m_device->m_scalePositionY;
		m_workspaceScale.z = m_device->m_scalePositionZ;
    }

}

//A virtual function must be implemented
void cHaptik3dofPointer::setDevice(cGenericDevice *a_device)
{
	//Assign an haptic device
	m_device = (cHaptikDevice*)a_device;
}

int cHaptik3dofPointer::initialize(const bool a_resetEncoders)
{
	int result = 0;

	if(a_resetEncoders){
    //Preliminary test on the device
	//Initialize (calibrate) device
	result = m_device->initialize(a_resetEncoders);
	if FAIL(result) return result;
	result = m_device->open();
    if FAIL(result) return result;
    updatePose();
	result = m_device->close();
    if FAIL(result) return result;
	}
    //Initialize only force models, no reset encoders
    unsigned int i;

    for (i=0; i<m_pointForceAlgos.size(); i++)
    {
        if (m_pointForceAlgos[i] != NULL)
        {
            m_pointForceAlgos[i]->initialize(m_world, m_deviceGlobalPos);
        }
    }

    return result;
}

int cHaptik3dofPointer::start()
{   
	int result = RET_VAL_SYSNOTAVAILABLE;

    //Initialize the device
	result = m_device->initialize();
	
	//If the system is not available
	//Cannot initialize the device
	if FAIL(result) return result;

	result = m_device->open();

    //Initialize communication with device
    return result;
}

int cHaptik3dofPointer::stop()
{
	int result = RET_VAL_SYSNOTAVAILABLE;

	result = m_device->close();

	//Stop device
    return result;
}

//Release the interface of the device
void cHaptik3dofPointer::releaseInterface(){

	m_device->release();

}

void cHaptik3dofPointer::setWorkspace(const double& a_workspaceAxisX,
    const double& a_workspaceAxisY, const double& a_workspaceAxisZ)
{
    m_halfWorkspaceAxisX = a_workspaceAxisX / 2.0;
    m_halfWorkspaceAxisY = a_workspaceAxisY / 2.0;
    m_halfWorkspaceAxisZ = a_workspaceAxisZ / 2.0;    
}

void cHaptik3dofPointer::updatePose()
{
    cVector3d pos;

    if (m_device == NULL) { return; }

    int result;

	//Read position from device
    if (m_useNormalizedPositions)
        result = m_device->command(CHAI_CMD_GET_POS_NORM_3D, &pos);
    else
        result = m_device->command(CHAI_CMD_GET_POS_3D, &pos);

    if (result != CHAI_MSG_OK) { return; }

  if (m_useNormalizedPositions == true)
    {
      //Scale from device workspace to virtual workspace
	  //(after scale device/maxworkspace). The ratio could not be
	  //unitary with the virtual device
      m_deviceLocalPos.set( m_halfWorkspaceAxisX * pos.x,
                            m_halfWorkspaceAxisY * pos.y,
                            m_halfWorkspaceAxisZ * pos.z);
    }
    else {
      m_deviceLocalPos.set(pos.x,pos.y,pos.z);
    }

    //Update global position
    cVector3d tPos;
    m_globalRot.mulr(m_deviceLocalPos, tPos);
    tPos.addr(m_globalPos, m_deviceGlobalPos);

    //Read orientation
    m_device->command(CHAI_CMD_GET_ROT_MATRIX, &m_deviceLocalRot);

    //Update global orientation
    m_deviceLocalRot.mulr(m_globalRot, m_deviceGlobalRot);

    //Read switch
    m_device->command(CHAI_CMD_GET_SWITCH_MASK, &m_button);
    
    //Read velocity (local coordinates)
    m_device->command(CHAI_CMD_GET_VEL_3D, &m_deviceLocalVel);

    //Update velocity (global coordinates)
    m_globalRot.mulr(m_deviceLocalVel, m_deviceGlobalVel);
}

void cHaptik3dofPointer::computeForces()
{
    unsigned int i;

    //Memory variables for forces
    cVector3d force;
    force.zero();
  
	//Compute force in world-coordinates for every force point of the algorithm
    for (i=0; i<m_pointForceAlgos.size(); i++)
    {
        if (m_pointForceAlgos[i] != NULL)
        {
            force.add(m_pointForceAlgos[i]->computeForces(m_deviceGlobalPos));
        }
    }

    //Update result
    m_lastComputedGlobalForce.copyfrom(force);
}

void cHaptik3dofPointer::applyForces()
{
    if (m_device == NULL) { return; }

    //Convert force in local coordinates of the device
    cMatrix3d tRot;
    m_globalRot.transr(tRot);
    tRot.mulr(m_lastComputedGlobalForce, m_lastComputedLocalForce);

    if (
      (m_waitForSmallForce == false)
      ||
      ((!m_forceStarted) && (m_lastComputedLocalForce.lengthsq() <0.000001))
      )
        m_forceStarted = true;

    //Apply forces
    if ((m_forceON) && (m_forceStarted))
    {
		//Send forces to the device
        m_device->command(CHAI_CMD_SET_FORCE_3D, &m_lastComputedLocalForce);

    }
    else
    {
		//No force send
        cVector3d ZeroForce = cVector3d(0.0, 0.0, 0.0);
        m_device->command(CHAI_CMD_SET_FORCE_3D, &ZeroForce);
    }
}

//Return the state of the button 0
int cHaptik3dofPointer::buttonStateZero(){

	int buttonState;
	m_device->command(CHAI_CMD_GET_SWITCH_0, &buttonState);

	return buttonState;
}

void cHaptik3dofPointer::render(const int a_renderMode)
{
    if (a_renderMode != CHAI_RENDER_MODE_NON_TRANSPARENT_ONLY && a_renderMode != CHAI_RENDER_MODE_RENDER_ALL)
      return;   

    //Render tool sphere
    glEnable(GL_COLOR_MATERIAL);
    glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);

    //OpenGL matrix
    cMatrixGL frameGL;

    //Compute local position of the proxy
    cProxyPointForceAlgo* default_proxy = getProxy();
    cVector3d proxyLocalPos;
    if (default_proxy != NULL)
    {
        proxyLocalPos = default_proxy->getProxyGlobalPosition();
        proxyLocalPos.sub(m_globalPos);
        cMatrix3d tRot;
        m_globalRot.transr(tRot);
        tRot.mul(proxyLocalPos);
    }

    //Proxy render
    if ((m_render_mode != RENDER_DEVICE) && (default_proxy != NULL))
    {
      //Button 0 defines proxy color
      if (m_button & 1)
      {
          glColor4fv(m_colorProxyButtonOn.pColor());        
      }
      else
      {
          glColor4fv(m_colorProxy.pColor());          
      }

      frameGL.set(proxyLocalPos);
      frameGL.glMatrixPushMultiply();
      cDrawSphere(m_displayRadius, 16, 16);
      frameGL.glMatrixPop();
    }
    
	//Device render
    if (m_render_mode != RENDER_PROXY)
    {
        if (m_button & 1) {
          glColor4fv(m_colorProxyButtonOn.pColor());
        }
        else {
          glColor4fv(m_colorDevice.pColor());
        }
        frameGL.set(m_deviceLocalPos);
        frameGL.glMatrixPushMultiply();
            cDrawSphere(m_displayRadius, 16, 16);
        frameGL.glMatrixPop();

        if (m_showToolFrame)
        {
            //Orientation render
            frameGL.set(m_deviceLocalPos, m_deviceLocalRot);
            frameGL.glMatrixPushMultiply();
            cDrawFrame(m_toolFrameSize);
            frameGL.glMatrixPop();
        }
    }

	//Render line
    if ((m_render_mode == RENDER_PROXY_AND_DEVICE) && (default_proxy != NULL))
    {
      glDisable(GL_LIGHTING);
      glLineWidth(1.0);
      glColor4fv(m_colorLine.pColor());
      glBegin(GL_LINES);
          glVertex3d(m_deviceLocalPos.x, m_deviceLocalPos.y, m_deviceLocalPos.z);
          glVertex3d(proxyLocalPos.x, proxyLocalPos.y, proxyLocalPos.z);
      glEnd();
      glEnable(GL_LIGHTING);
    }    

}

void cHaptik3dofPointer::setRadius(const double& a_radius)
{
    m_displayRadius = a_radius;

        //Update radius of the collision detector
		cProxyPointForceAlgo* default_proxy = getProxy();
		if (default_proxy != NULL)
			default_proxy->setProxyRadius(a_radius * CHAI_SCALE_PROXY_RADIUS);
}

cProxyPointForceAlgo* cHaptik3dofPointer::getProxy()
{
	  for (unsigned int i=0; i<m_pointForceAlgos.size(); i++)
		{
		  cProxyPointForceAlgo* default_proxy = dynamic_cast<cProxyPointForceAlgo*>(m_pointForceAlgos[i]);
		  if (default_proxy != NULL)
			  return default_proxy;
		}
	  return NULL;
}

void cHaptik3dofPointer::setToolFrame(const bool& a_showToolFrame, const double& a_toolFrameSize)
{
    m_showToolFrame = a_showToolFrame;
    m_toolFrameSize = a_toolFrameSize;
}

int cHaptik3dofPointer::setForcesON()
{
    if (!m_forceON)
    {
          m_forceStarted = false;
          m_forceON = true;
    }
    return 0;
}

int cHaptik3dofPointer::setForcesOFF()
{
    m_forceON = false;
    return 0;
}

cVector3d cHaptik3dofPointer::getWorkspaceScaleFactors() {
    double scale;
    int result = m_device->command(CHAI_CMD_GET_NORMALIZED_SCALE_FACTOR, &scale);
    if (result != CHAI_MSG_OK) { return cVector3d(0.0,0.0,0.0); }  
  
    cVector3d toReturn(1.0/m_halfWorkspaceAxisX,1.0/m_halfWorkspaceAxisY,1.0/m_halfWorkspaceAxisZ);
    toReturn *= scale;
    return toReturn;
}

const Char8* cHaptik3dofPointer::readDeviceInfoFromLibrary(int a_in, int a_char){
 
	const Char8* out;

	switch(a_char){

		case DEVICE_NAME:
			out = m_device->getPointerToLibrary()->device[a_in].name;
        break;

		case DEVICE_MANUFACTURER:
			out = m_device->getPointerToLibrary()->device[a_in].manufacturer;
		break;
		
		case DEVICE_MODEL:
			out = m_device->getPointerToLibrary()->device[a_in].model;
		break;

	    default: out = "INFO NOT AVAILABLE";
	}

	return out;
}

//Read particular device informations
const Char8* cHaptik3dofPointer::readDeviceInfo(int a_char){
 
	const Char8* out;

	switch(a_char){

		case DEVICE_NAME:
	        out = m_device->getName();
        break;

		case DEVICE_MANUFACTURER:
			out = m_device->getManufacturer();
		break;
		
		case DEVICE_MODEL:
			out = m_device->getModel();
		break;

	    default: out = "INFO NOT AVAILABLE";
	}

	return out;
}


//Get the result strings
const Char8* cHaptik3dofPointer::getResultToString(int a_result){

	return m_device->resultToString(a_result);

}